#!/usr/bin/python
# -*- coding: UTF-8 -*-
import rospy
from std_msgs.msg import String
from robot_by_fe3.msg import location
import cv2
import serial
import serial.tools.list_ports
import time
from pyzbar import pyzbar
import numpy as np

class robot_funtion():
    def __init__(self):
        self.ser = serial.Serial("/dev/ttyUSB0", 115200, bytesize=8, stopbits=1, timeout=0.01)
        self.cap = cv2.VideoCapture(0)
        self.cap.set(3, 320)
        self.cap.set(4, 240)
        # 蓝色
        self.colorLow = np.array([100, 43, 46])
        self.colorHigh = np.array([124, 255, 255])
        self.colorLowh = np.array([0, 0, 0])
        self.colorHighh = np.array([180, 255, 43])

        self.location_data = []
        self.flag = 'STOP'
        rospy.init_node('Robot_Fe3', anonymous=True)
        self.state_pub = rospy.Publisher('robot_state', String, queue_size=10)
        rospy.Subscriber('location', location, self.get_location_label)
        rospy.Subscriber('fe_robot_action', String, self.robot_action)
        rospy.Subscriber('fe_robot_stop', String, self.robot_stop)
        pass

    def get_location_label(self,data):
        if data.location_label == 'location_label':
            self.location_data[:] = []
        else:
            self.location_data.append(data)
            print self.location_data

    def robot_action(self,data):
        self.flag = data.data

    def robot_stop(self,data):
        self.flag = data.data

    def callback(self,a):
        return a[0]

    def get_QR_code(self):
        ret, img = self.cap.read()
        hsv_img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
        img2 = cv2.inRange(hsv_img, self.colorLow, self.colorHigh)
        thres = cv2.inRange(hsv_img, self.colorLow, self.colorHigh)
        #gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        #RET, thres = cv2.threshold(gray, 80, 255, cv2.THRESH_BINARY)  # CV_THRESH_BINARY
        imga, contours, hierarchy = cv2.findContours(img2, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        if len(contours) != 0:
            data = []
            for i in contours:
                M = cv2.moments(i)
                if M['m00'] != 0:
                    cx = int(M['m10'] / M['m00'])
                    cy = int(M['m01'] / M['m00'])
                    # dis = ((cx - flag_x) * (cx - flag_x) + (cy - flag_y) * (cy - flag_y)) ** 0.5
                    data.append([M['m00'], cx, cy])
            if len(data) != 0:
                data.sort(key=self.callback, reverse=True)
                print(data[0])
                if data[0][0] >= 200 and data[0][0] <= 6000:
                    send_data = str(data[0][1]) + ";" + str(data[0][2]) + ";"
                    self.ser_send("D", 2, send_data)

                # pub.publish(str(data[0]))
        test = pyzbar.decode(thres)
        cv2.imshow("thres", thres)
        cv2.imshow("img", img)
        cv2.imshow("img2", img2)
        testdate = ''
        for tests in test:
            testdate = tests.data.decode('utf-8')
            print(testdate)
        print 'len(testdate)=',len(testdate)
        if len(testdate) != 0:
            for i in range(len(self.location_data)):
                if testdate in self.location_data[i].location_label:
                    self.state_pub.publish(testdate)
                    self.ser_send("I", 1, self.location_data[i].direction)
                    time.sleep(0.15)
                    break
        # if testdate == "right":
        #     ser_send("I", 1, "RIGHT;")
        #     time.sleep(0.15)
        #     pass
        # if testdate == "left":
        #     ser_send("I", 1, "LEFT;")
        #     time.sleep(0.15)
        #     pass
        # if testdate == "stop":
        #     ser_send("I", 1, "STOP;")
        #     time.sleep(0.15)
        #     pass

    def ser_send(self,cmd, data_num, data):
        # 通信协议  数据包
        # hand        包头 TR
        # cmd_id      指令/数据选择位（I为指令/D为数据）
        # cmd         指令/数据个数（一个字节）
        # 指令或数据加分号
        # 校验位（校验位占两字节，前面所有字符的个数）
        # 结束符 \n
        hand = "TR"
        cmd_copy = hand + str(cmd) + str(data_num) + str(data)
        if len(cmd_copy) <= 9:
            send_com = cmd_copy + "0" + str(len(cmd_copy)) + "\n"
        else:
            send_com = cmd_copy + str(len(cmd_copy)) + "\n"
        self.ser.write(send_com.encode())
        print(send_com)







if __name__ == "__main__":
    my_robot=robot_funtion()
    while True:
        if my_robot.flag == 'STOP':
            my_robot.ser_send("I", 1, "STOP;")
            continue
        else:
            my_robot.get_QR_code()
        key=cv2.waitKey(1)
        if key == ord('a'):
            my_robot.ser_send("I", 1, "STOP;")
            break
    # try:
    #     while True:
    #         get_QR_code()
    #         key = cv2.waitKey(1)
    #         if key == ord('a'):
    #             ser_send("I", 1, "STOP;")
    #             break
    # except Exception as e:
    #     print(e)
